#include "formation_avoidance/formation.h"

using namespace std;
using namespace Eigen;

int main (int argc, char** argv) 
{   
    ros::init(argc, argv, "task");
    ros::NodeHandle nh("~");

    const int LOOPRATE = 30;
    ros::Rate loop_rate(LOOPRATE);  
    ROS_INFO("task node start-----");

    Eigen::Vector3f start_p(2.8,0,1.8);
    Eigen::Vector3f end_p(6,0,1.8);

    Formation_Agent agent;
    agent.init(nh);

    for(int i=0;i<50;i++)
    {
        ros::spinOnce();
        loop_rate.sleep();
    }

    for(int i=0;i<5;i++)
    {
        if(agent.a_star_search(start_p,end_p)) break;
        loop_rate.sleep();

    }
   
	return 0;

}
